
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_state.c
  * @author     baiyang
  * @date       2021-8-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <notify/notify.h>
#include <ahrs/ahrs_view.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static uitc_actuator_armed actuator_armed;
static uitc_vehicle_home vehicle_home;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void fms_set_auto_armed(bool b)
{
    // if no change, exit immediately
    if( fms.ap.auto_armed == b )
        return;

    fms.ap.auto_armed = b;
    if(b){
        //AP::logger().Write_Event(LogEvent::AUTO_ARMED);
    }
}

/**
  * @brief       
  * @param[in]   b  
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_actuator_armed_notify(bool b)
{
    uint64_t timestamp_us = time_micros64();

    notify_flags.armed = b;

    brd_set_soft_armed(b);

    if (b) {
        vehicle_home.timestamp_us = timestamp_us;

        vehicle_home.valid_alt = fms.ahrs->valid_alt;
        vehicle_home.valid_hpos = fms.ahrs->valid_hpos;
        vehicle_home.valid_lpos = fms.ahrs->valid_lpos;

        vehicle_home.alt = fms.ahrs->curr_alt_amsl;
        vehicle_home.z   = fms.ahrs->home_alt_above_origin;

        vehicle_home.lat = fms.ahrs->valid_hpos ? fms.ahrs->curr_loc.lat : 0;
        vehicle_home.lon = fms.ahrs->valid_hpos ? fms.ahrs->curr_loc.lng : 0;

        vehicle_home.x = fms.ahrs->relpos_cm.x * 0.01f;
        vehicle_home.y = fms.ahrs->relpos_cm.y * 0.01f;

        vehicle_home.yaw = fms.ahrs->yaw;
        vehicle_home.mag_decl = fms.ahrs->valid_hpos ? geo_get_declination((double)vehicle_home.lat*1e-7, (double)vehicle_home.lon*1e-7) : 0;

        itc_publish(ITC_ID(vehicle_home), &vehicle_home);
    }

    actuator_armed.timestamp_us = timestamp_us;
    actuator_armed.armed = b;
    itc_publish(ITC_ID(vehicle_actuator_armed), &actuator_armed);
}

/*------------------------------------test------------------------------------*/


